#include "motor.h"
#using <System.dll>
using namespace System;
using namespace System::IO;
using namespace System::Net;
using namespace System::Net::Sockets;
using namespace System::Collections::Generic;
using namespace System::Text;
using namespace System::Xml;
using namespace System::Threading;

#define GUID "79c0eb8e408dd2c3b747a58bfe4f51e2b7125631" //global unique id
#define BROADCAST_SERVER "pave.cs.pdx.edu"	//server ipv4 address
#define BROADCAST_PORT 2312 //server port to knock
#define VERSION "0.2"
/* Default angle and distance. */
#define ANG_DEFAULT 90		//default turn 90 degrees
#define DIST_DEFAULT 300		//default distance to move 30cm
#define SPEED_DEFAULT 100		//default speed 10cm /s
#define RATE_DEFAULT 0.1


/* Global variables and defines for bump sensors. */
#define BUMP_BELLY_PORT IN_2
#define BUMP_BACK_PORT IN_3
#define BUMP_FRONT_PORT IN_4

//sensor variables
int bumpFront = 0;
int bumpBack = 0;
int bumpBelly = 0;

//forward declaration of functions
void writeSocket(TcpClient^ client, String ^ message); //write socket methd
int bump(Comm::NXTComm* comm);	//method to see if we bumped
String^ readSocket(TcpClient^ client);	//read socket method
static String^ parse_commands(String^ the_instructions); //xml parser
static String^ initial_message();	//initiatl message to get server authentication	
String^ package_sensor_data(String^ the_values); //needed to keep session alive..not sending data to pave yet

int main(int argc, char** argv)
{
	using namespace std;
	ShrMotor shrrm;
	TcpClient^ client = gcnew TcpClient( BROADCAST_SERVER,BROADCAST_PORT ); //Connect to pave
	writeSocket(client,initial_message());
	Console::WriteLine("Connecting to Schroddinger...give me a sec!");
	NXT::OpenBT(&shrrm.comm); //connect to NXT via bluetooth or USB if USB use Open(..)
	cout << "Initializing the NXT" << endl;
	// Set up the HiTechnic motor controller. 
	shrrm.setup(IN_1);
	/* Configure the bump sensors. */
	NXT::Sensor::SetTouch(&shrrm.comm,BUMP_FRONT_PORT);
	NXT::Sensor::SetTouch(&shrrm.comm,BUMP_BACK_PORT);
	NXT::Sensor::SetTouch(&shrrm.comm,BUMP_BELLY_PORT);
	
	double battery; //battery variable..maybe send back to pave?
	
	int angDist = 0; // Angle in degrees or distance in centimeters.
	double spdRate = 0;
	int delayCount = 0;
	int bumped=0;
	while (true) //forever loop
	{
		// Get instructions from middleware
		String^ instructions = readSocket(client);
		String^ parsed = String::Empty;
		parsed = parse_commands(instructions);
		Console::WriteLine(parsed);
		if(parsed->Contains("forward"))
		{
			if (angDist == 0) 
				angDist = DIST_DEFAULT;
			if (spdRate == 0) 
				spdRate = SPEED_DEFAULT;
			cout << "Go shroddie!!" << endl;
			shrrm.resetPosition();
			for(int i=0;i<5;i++ )
			{
				bumped = bump(&shrrm.comm);
				if(!bumped)
				{
					shrrm.drive(spdRate); 
					Thread::Sleep(1000);
				}else
				{
					Console::WriteLine("Got bumped");
				}
			}
		}
		if(parsed->Contains("back"))
		{
			if (angDist == 0) 
				angDist = DIST_DEFAULT;
			if (spdRate == 0) 
				spdRate = SPEED_DEFAULT;
			cout << "Go shroddie!!" << endl;
			shrrm.resetPosition();
			for(int i=0;i<5;i++ )
			{
				bumped = bump(&shrrm.comm);
				if(!bumped)
				{
					shrrm.drive(spdRate); 
					Thread::Sleep(1000);
				}else
				{
					Console::WriteLine("Got bumped");
				}
			}
		}

		if(parsed->Contains("right"))
		{

		}
		// Package up sensor data as XML and send to middleware
		writeSocket(client, package_sensor_data("\n"));
	}
	
	char r;
	cin >> r;
    return 0;
}

void writeSocket(TcpClient^ client, String ^ message)
{
	// Translate the passed message into ASCII and store it as a Byte array.
	array<Byte>^data = Text::Encoding::ASCII->GetBytes( message );
	// Get a client stream for reading and writing.
	//  Stream stream = client->GetStream();
	NetworkStream^ stream = client->GetStream();
	// Send the message to the connected TcpServer. 
	stream->Write( data, 0, data->Length );
	stream->Flush();
}


void disconnect(TcpClient^ client) 
{
	client->GetStream()->Close();
	client->Close();
	Console::WriteLine("Disconnected\n");
}

String^ readSocket(TcpClient^ client) 
{
	String^ str; // = String::Empty;
	NetworkStream^ ns = client->GetStream();
	StringBuilder^ sb = gcnew StringBuilder();
	while (!(sb->ToString()->Contains("</instructions>"))) 
	{
		int ch = ns->ReadByte();
		if (ch == -1) 
			break;

		char c = (char)ch;
		wchar_t w = c;
		sb->Append(w);
	}
	str = sb->ToString();
	Console::WriteLine(str);
	return str;
}

int bump(Comm::NXTComm* comm) 
{
	int bumped = 0;
	if (NXT::Sensor::GetValue(comm, BUMP_FRONT_PORT)) {
	bumpFront = 1;
	bumped = 1;
	}
	if (NXT::Sensor::GetValue(comm, BUMP_BELLY_PORT)) {
	bumpBelly = 1;
	bumped = 1;
	}
	if (NXT::Sensor::GetValue(comm, BUMP_BACK_PORT)) {
	bumpBack = 1;
	bumped = 1;
	}
	return bumped;
}

static String^ initial_message() 
{
    String ^ init = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" +
        "<init><guid>" + GUID + "</guid>" +
        "<version>" + VERSION + "</version></init>";
	return init;
}


static String^ parse_commands(String^ the_instructions) 
{
	StringBuilder^ sb = gcnew StringBuilder();
	List <String^> list = gcnew List<String^>();
	sb->EnsureCapacity(60000);
	if (the_instructions->Contains("<instructions></instructions>")) {
		return "getsensorvalues\n";
	}

	bool seen_name = false;
	StringReader^ sr = gcnew StringReader(the_instructions);
	XmlReader^ reader = XmlReader::Create(sr);
	try
	{
		while (reader->Read()) {
			if (reader->IsStartElement()) {
				String^ name = reader->Name;
				if (name->Equals("name") || name->StartsWith("arg")) {
					if (name->Equals("name") && seen_name) sb->Append("\n");
					seen_name = true;
					if (reader->Read()) {
						sb->Append(reader->Value);
						if (!reader->Value->Equals("")) sb->Append(" ");
					}
				}
			}
		}
	}
	catch(Exception^ e)
	{
		Console::WriteLine(e);
	}
	if (seen_name) sb->Append("\n");
	sb->Append("getsensorvalues\n");

	String^ str = sb->ToString();
	return str->Replace(" \n", "\n"); // Remove extra spaces
}

static String^ package_sensor_data(String^ the_values) 
{
	StringBuilder^ sb = gcnew StringBuilder();
	sb->Append("<?xml version=\"1.0\" encoding=\"UTF-8\"?><data>");

	the_values = the_values->Replace("\n", "");
	/*String ^[] stk = the_values.Split(' ');
	for (int i = 0; i < stk.Length; i++) {
		sb->Append("<sensor><id>");
		sb->Append(i);
		sb->Append("</id><value>");
		sb->Append(stk[i]);
		sb->Append("</value></sensor>");
	}*/
	sb->Append("</data>");

	return sb->ToString();
}
